#include "yavrsOmega.h"

yavrsEventListener::yavrsEventListener(std::string hostname)
{
	struct addrinfo hints, *servinfo, *p;
	int rv;
	int numbytes;

	memset(&hints, 0, sizeof hints);
	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_flags = AI_PASSIVE;

	if((rv = getaddrinfo(hostname.c_str(), msgPort, &hints, &servinfo)) != 0) {
		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
	}

	for(p = servinfo; p!=NULL; p = p->ai_next) {
		if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1) {
			perror("yavrsOmega:");
			continue;
		}

		if(connect(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
			close(sockfd);
			perror("yavrsOmega");
			continue;
		}
		break;
	}

	freeaddrinfo(servinfo);

	if(p == NULL) {
		fprintf(stderr, "yavrsOmega: failed to connect to %s\n", hostname.c_str());
		return;
	}


	//send handshake
	if((numbytes = send(sockfd, "data_on,27001", strlen("data_on,27001"), 0)) == -1) {
		perror("sendto");
		return;
	}

	close(sockfd);


	//update hints to switch from tcp to udp
	hints.ai_socktype = SOCK_DGRAM;

	if((rv = getaddrinfo(NULL, dataPort, &hints, &servinfo)) != 0) {
		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
	}

	for(p = servinfo; p != NULL; p = p->ai_next) {
		if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1) {
			perror("yavrsOmega: socket");
			continue;
		}

		if(bind(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
			close(sockfd);
			perror("yavrsOmega: bind");
			continue;
		}

		break;
	}

	freeaddrinfo(servinfo);

	if(p == NULL) {
		fprintf(stderr, "failed to bind socket\n");
		return;
	}

	fcntl(sockfd, F_SETFL, O_NONBLOCK); // make socket non-blocking
}

yavrsEventListener::~yavrsEventListener()
{
	close(sockfd);
}

inline void yavrsNunchuk(yavrsEvent *event, int field, int intValue, float floatValue)
{
	switch(field) {
		case 3: // analox x
			event->data.nunchuk.analogX = intValue;
			break;
		case 4: // analog y
			event->data.nunchuk.analogY = intValue;
			break;
		case 5: // accel x
			event->data.nunchuk.accelX = intValue;
			break;
		case 6: // accel y
			event->data.nunchuk.accelY = intValue;
			break;
		case 7: // accel z
			event->data.nunchuk.accelZ = intValue;
			break;
		case 8: // c button
			event->data.nunchuk.c = intValue;
			break;
		case 9: // z button
			event->data.nunchuk.z = intValue;
			break;
		default:
			printf("Too many fields for nunchuk (%d)\n", field);
			break;
	}
}

inline void yavrsWiimote(yavrsEvent *event, int field, int intValue, float floatValue)
{
	switch(field) {
		case 3: // battery
			break;
		case 4: // x accel
			event->data.wiimote.accelX = intValue;
			break;
		case 5: // y accel
			event->data.wiimote.accelY =  intValue;
			break;
		case 6: // z accel
			event->data.wiimote.accelZ = intValue;
			break;
		case 7: // a button
			event->data.wiimote.a = intValue;
			break;
		case 8: // b button 
			event->data.wiimote.b = intValue;
			break;
		case 9: // plus button
			event->data.wiimote.plus = intValue;
			break;
		case 10: // home button
			break;
		case 11: // minus button
			event->data.wiimote.minus = intValue;
			break;
		case 12: // one button
			break;
		case 13: // two button
			break;
		case 14: // up
			event->data.wiimote.up = intValue;
			break;
		case 15: // down
			event->data.wiimote.down = intValue;
			break;
		case 16: // left
			event->data.wiimote.left = intValue;
			break;
		case 17: // right
			event->data.wiimote.right = intValue;
			break;
		case 18: // ir mode
		case 19: // ir0 valid
		case 20: // ir0 x
		case 21: // ir0 y
		case 22: // ir1 valid
		case 23: // ir1 x
		case 24: // ir1 y
		case 25: // ir2 valid
		case 26: // ir2 x
		case 27: // ir2 y
		case 28: // ir3 valid
		case 29: // ir3 x
		case 30: // ir3 y
			break;
		default:
			printf("Too many fields for Wiimote (%d)\n", field);
			break;
	}
}

inline void yavrsMocap(yavrsEvent *event, int field, int intValue, float floatValue)
{
	switch(field) {
		case 2:
			event->data.mocap.x = floatValue;
			break;
		case 3:
			event->data.mocap.y = floatValue;
			break;
		case 4:
			event->data.mocap.z = floatValue;
			break;
		case 5:
			event->data.mocap.q.x = floatValue;
			break;
		case 6:
			event->data.mocap.q.y = floatValue;
			break;
		case 7:
			event->data.mocap.q.z = floatValue;
			break;
		case 8:
			event->data.mocap.q.w = floatValue;
			break;
		default:
			printf("Incorrect  format\n");
			break;
	}
}

inline int parseNumber(int position, char * string, int * intValue, float * floatValue)
{

}

yavrsEvent yavrsEventListener::parseEvent()
{
	int i=0, currentInt=0;
	float currentFloat=0.0;
	std::string number = "";
	bool isFloat = false; // assume int until we find a decimal point
	int serviceType = -1;
	int field = 0;
	yavrsEvent event;

	memset(&event, 0, sizeof(event)); // initialize event with all 0's

	//printf("%s\n", buffer);

	for(i=0; i<numBytesRecvd; i++) {
		switch(buffer[i]) {
			case '0':
			case '1':
			case '2':
			case '3':
			case '4':
			case '5':
			case '6':
			case '7':
			case '8':
			case '9':
				number += buffer[i];
				break;
			case '.':
				number += buffer[i];
				isFloat = true;
				break;
			case '-':
				number += buffer[i];
				break;
			case ',':
			case ' ':
			case ':':
			case '\0':
				if(isFloat) {
					currentFloat = atof(number.c_str());
				} else {
					currentInt = atoi(number.c_str());
				}
				isFloat = false;
				number = "";

				if(field == 0) {
					serviceType = currentInt;
					switch(currentInt) {
						case MOCAP:
							event.type = YAVRS_MOCAP_EVENT;
							break;
						case CONTROLLER:
							break;
						default:
							break;
					}
					field++; // increment field here since we're breaking
					break;
				} else if(field == 1) {
					event.sourceID = currentInt;
					field++;
					break;
				}

				switch(serviceType) {
					case POINTER:
						break;
					case MOCAP:
						yavrsMocap(&event, field, currentInt, currentFloat);
						break;
					case KEYBOARD:
						break;
					case CONTROLLER:
						if(field == 2) {
							if(currentInt == WIIMOTE) {
								event.type = YAVRS_WIIMOTE_EVENT;
							} else if(currentInt == WII_NUNCHUK) {
								event.type = YAVRS_NUNCHUK_EVENT;
							}
						} else {
							if(event.type == YAVRS_WIIMOTE_EVENT) {
								yavrsWiimote(&event, field, currentInt, currentFloat);
							} else if(event.type == YAVRS_NUNCHUK_EVENT) {
								yavrsNunchuk(&event, field, currentInt, currentFloat);
							}
						}
						break;
					case UI:
						break;
					case GENERIC:
						break;
					default:
						printf("Unknown device type\n");
						break;
				}
				field++;
				break;
			default:
				printf("Unexpected character %c at %d\n", buffer[i], i);
				break;
		}
	}
	eventQueue.push(event);
}

bool yavrsEventListener::getNextEvent(yavrsEvent *event)
{
	addr_len = sizeof(their_addr);
	while((numBytesRecvd = recvfrom(sockfd, buffer, MAXBUFSIZE-1, 0, (struct sockaddr *)&their_addr, &addr_len)) > 0) {
		buffer[numBytesRecvd-1] = '\0';
		parseEvent();
	}

	if(eventQueue.size() > 0) {
		*event = eventQueue.front();
		eventQueue.pop();
		return true;
	} else {
		return false;
	}
}


